#include <ros/ros.h>
#include <std_msgs/Int32MultiArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <vector>
#include <tuple>
#include <iostream>
#include <iomanip>

class PathProcessor
{
public:
    PathProcessor()
    {
        sub_ = nh_.subscribe("path_topic", 10, &PathProcessor::pathCallback, this);
        path_pub_ = nh_.advertise<nav_msgs::Path>("visual_path", 1, true);
        processed_ = false;
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    ros::Publisher path_pub_;

    std::vector<std::tuple<float, float, float>> waypoint_;  // (x, y, z)
    bool processed_;  // 标志是否已经处理过

    void pathCallback(const std_msgs::Int32MultiArray::ConstPtr& msg)
    {
        if (processed_)
            return;

        const std::vector<int>& data = msg->data;

        int start_idx = -1;
        int end_idx = -1;

        for (size_t i = 0; i < data.size() - 1; ++i)
        {
            if (data[i] == -2 && data[i + 1] == -2)
                start_idx = i + 2;
            if (data[i] == -9 && data[i + 1] == -9)
            {
                end_idx = i;
                break;
            }
        }

        if (start_idx == -1 || end_idx == -1 || end_idx <= start_idx)
        {
            ROS_WARN("Invalid frame. Skipping...");
            return;
        }

        waypoint_.clear();

        for (int i = start_idx; i + 1 < end_idx; i += 2)
        {
            int x_raw = data[i];
            int y_raw = data[i + 1];

            if (x_raw < 0 || y_raw < 0)
                continue;

            float x = x_raw * 0.5f;
            float y = y_raw * 0.5f;
            float z = 1.2f;

            waypoint_.emplace_back(x, y, z);
        }

        ROS_INFO("Received %lu waypoints. Publishing once.", waypoint_.size());
        printWaypoints();
        publishPath();

        processed_ = true;
    }

    void printWaypoints()
    {
        std::cout << "Waypoint list:\n";
        for (size_t i = 0; i < waypoint_.size(); ++i)
        {
            const auto& wp = waypoint_[i];
            std::cout << std::fixed << std::setprecision(2)
                      << "[" << i << "]: (x=" << std::get<0>(wp)
                      << ", y=" << std::get<1>(wp)
                      << ", z=" << std::get<2>(wp) << ")\n";
        }
    }

    void publishPath()
    {
        nav_msgs::Path path_msg;
        path_msg.header.stamp = ros::Time::now();
        path_msg.header.frame_id = "map";  // RViz 中设置 Fixed Frame 为 map

        for (const auto& wp : waypoint_)
        {
            geometry_msgs::PoseStamped pose;
            pose.header = path_msg.header;
            pose.pose.position.x = std::get<0>(wp);
            pose.pose.position.y = std::get<1>(wp);
            pose.pose.position.z = std::get<2>(wp);
            pose.pose.orientation.w = 1.0;  // 无旋转
            path_msg.poses.push_back(pose);
        }

        path_pub_.publish(path_msg);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "path_processor_node");
    PathProcessor pp;
    ros::spin();
    return 0;
}

